///\todo upgrade

//////////////////////////////////////////////////////////////////////////////////
//                                                                              //
//  This file is part of the buola project (https://code.google.com/p/buola/).  //
//                                                                              //
//  Copyright(c) 2007-2012 Xavi Gratal                                          //
//  gratal AT gmail DOT com                                                     //
//                                                                              //
//  Buola is free software: you can redistribute it and/or modify               //
//  it under the terms of the GNU General Public License as published by        //
//  the Free Software Foundation, either version 3 of the License, or           //
//  (at your option) any later version.                                         //
//                                                                              //
//  Buola is distributed in the hope that it will be useful,                    //
//  but WITHOUT ANY WARRANTY; without even the implied warranty of              //
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               //
//  GNU General Public License for more details.                                //
//                                                                              //
//  You should have received a copy of the GNU General Public License           //
//  along with buola.  If not, see <http://www.gnu.org/licenses/>.              //
//                                                                              //
//////////////////////////////////////////////////////////////////////////////////

#ifndef _BUOLA_MAT_OPTIMIZE_H_
#define _BUOLA_MAT_OPTIMIZE_H_

#include <buola/mat.h>
#include <gsl/gsl_multimin.h>

namespace buola { namespace mat {

namespace detail {
    
    template<typename tFunc>
    class CGSLFAdapter
    {
    public:
        CGSLFAdapter(tFunc &pFunc,size_t pR,size_t pC)
            :   mFunc(pFunc)
            ,   mR(pR)
            ,   mC(pC)
        {}
        
        static double Func(const gsl_vector *pVector,void *pClosure)
        {
            CGSLFAdapter *lAdapter=(CGSLFAdapter*)pClosure;

            return lAdapter->CallFunc(pVector);
        }
        
    private:
        double CallFunc(const gsl_vector *pVector)
        {
            mat::CMat_d lMat(mR,mC);
            
            for(int r=0;r<mR;r++)
                for(int c=0;c<mC;c++)
                    lMat(r,c)=gsl_vector_get(pVector,r*mC+c);
            
            return mFunc.F(lMat);
        }
        
    private:
        tFunc &mFunc;
        size_t mR,mC;
    };

    template<typename tFunc>
    class CGSLFDFAdapter
    {
    public:
        CGSLFDFAdapter(tFunc &pFunc,size_t pR,size_t pC)
            :   mFunc(pFunc)
            ,   mR(pR)
            ,   mC(pC)
        {}
        
        static double FFunc(const gsl_vector *pVector,void *pClosure)
        {
            CGSLFDFAdapter *lAdapter=(CGSLFDFAdapter*)pClosure;

            return lAdapter->CallFFunc(pVector);
        }
        
        static void DFFunc(const gsl_vector *pVector,void *pClosure,gsl_vector *pDF)
        {
            CGSLFDFAdapter *lAdapter=(CGSLFDFAdapter*)pClosure;

            lAdapter->CallDFFunc(pVector,pDF);
        }
        
        static void FDFFunc(const gsl_vector *pVector,void *pClosure,double *pF,gsl_vector *pDF)
        {
            CGSLFDFAdapter *lAdapter=(CGSLFDFAdapter*)pClosure;

            *pF=lAdapter->CallFDFFunc(pVector,pDF);
        }
        
    private:
        double CallFFunc(const gsl_vector *pVector)
        {
            mat::CMat_d lMat(mR,mC);
            
            for(int r=0;r<mR;r++)
                for(int c=0;c<mC;c++)
                    lMat(r,c)=gsl_vector_get(pVector,r*mC+c);
            
            return mFunc.F(lMat);
        }
        
        void CallDFFunc(const gsl_vector *pVector,gsl_vector *pDF)
        {
            mat::CMat_d lMat(mR,mC);
            
            for(int r=0;r<mR;r++)
                for(int c=0;c<mC;c++)
                    lMat(r,c)=gsl_vector_get(pVector,r*mC+c);
            
            mat::CMat_d lDF(mR,mC);
            lDF=mFunc.DF(lMat);
            
            for(int r=0;r<mR;r++)
                for(int c=0;c<mC;c++)
                    gsl_vector_set(pDF,r*mC+c,lDF(r,c));
        }
        
        double CallFDFFunc(const gsl_vector *pVector,gsl_vector *pDF)
        {
            mat::CMat_d lMat(mR,mC);
            
            for(int r=0;r<mR;r++)
                for(int c=0;c<mC;c++)
                    lMat(r,c)=gsl_vector_get(pVector,r*mC+c);

            double lF;
            mat::CMat_d lDF(mR,mC);
                
            std::tie(lF,lDF)=mFunc.FDF(lMat);
            
            for(int r=0;r<mR;r++)
                for(int c=0;c<mC;c++)
                    gsl_vector_set(pDF,r*mC+c,lDF(r,c));
            
            return lF;
        }
        
    private:
        tFunc &mFunc;
        size_t mR,mC;
    };

/*namespace detail*/ }

template<typename tFunc>
CMat_d minimize_gsl_f(const CMat_d &pInitialGuess,tFunc &pFunc,double pStepSize,double pEpsAbs)
{
    detail::CGSLFAdapter<tFunc> lAdapter(pFunc,pInitialGuess.Rows(),pInitialGuess.Cols());
    
    gsl_multimin_function lFunc;
    lFunc.f=detail::CGSLFAdapter<tFunc>::Func;
    lFunc.n=pInitialGuess.size();
    lFunc.params=&lAdapter;

    gsl_vector *lX=gsl_vector_alloc(pInitialGuess.size());
    gsl_vector *lStepSize=gsl_vector_alloc(pInitialGuess.size());
    for(int r=0;r<pInitialGuess.Rows();r++)
    {
        for(int c=0;c<pInitialGuess.Cols();c++)
        {
            gsl_vector_set(lX,r*pInitialGuess.Cols()+c,pInitialGuess(r,c));
            gsl_vector_set(lStepSize,r*pInitialGuess.Cols()+c,pStepSize);
        }
    }

    const gsl_multimin_fminimizer_type *lMinType=gsl_multimin_fminimizer_nmsimplex;
    gsl_multimin_fminimizer *lMinimizer=gsl_multimin_fminimizer_alloc(lMinType,pInitialGuess.size());
    gsl_multimin_fminimizer_set(lMinimizer,&lFunc,lX,lStepSize);

    for(int lIter=0;lIter<100000;lIter++)
    {
        int lStatus=gsl_multimin_fminimizer_iterate(lMinimizer);
        if(lStatus==27)
        {
            msg_info() << "no more progress after " << lIter << " iterations\n";
            break;
        }

        if(lStatus!=GSL_SUCCESS)
            throw XProcess("error "+to_string(lStatus)+"in fminimizer_iterate");
        lStatus=gsl_multimin_test_size(lMinimizer->size,pEpsAbs);
        if(lStatus!=GSL_CONTINUE)
        {
            if(lStatus==GSL_SUCCESS)
            {
                msg_info() << "optimization converged after " << lIter << " iterations\n";
                break;
            }
            else if(lStatus==27)
            {
                msg_info() << "no more progress after " << lIter << " iterations\n";
                break;
            }
                
            throw XProcess("error "+to_string(lStatus)+"in gsl_testgradient");
        }
    }

    mat::CMat_d lResult(pInitialGuess.Rows(),pInitialGuess.Cols());
    
    for(int r=0;r<lResult.Rows();r++)
        for(int c=0;c<lResult.Cols();c++)
            lResult(r,c)=gsl_vector_get(lMinimizer->x,r*lResult.Cols()+c);

    msg_info() << "final optimization value: " << lMinimizer->fval << "\n";

    gsl_multimin_fminimizer_free(lMinimizer);
    gsl_vector_free(lX);
    gsl_vector_free(lStepSize);
    
    return lResult;
}
    
template<typename tFunc>
CMat_d minimize_gsl_fdf(const CMat_d &pInitialGuess,tFunc &pFunc,double pStepSize,double pTolerance,double pEbsAbs)
{
    detail::CGSLFDFAdapter<tFunc> lAdapter(pFunc,pInitialGuess.Rows(),pInitialGuess.Cols());
    
    gsl_multimin_function_fdf lFunc;
    lFunc.f=detail::CGSLFDFAdapter<tFunc>::FFunc;
    lFunc.df=detail::CGSLFDFAdapter<tFunc>::DFFunc;
    lFunc.fdf=detail::CGSLFDFAdapter<tFunc>::FDFFunc;
    lFunc.n=pInitialGuess.size();
    lFunc.params=&lAdapter;

    gsl_vector *lX=gsl_vector_alloc(pInitialGuess.size());
    for(int r=0;r<pInitialGuess.Rows();r++)
        for(int c=0;c<pInitialGuess.Cols();c++)
            gsl_vector_set(lX,r*pInitialGuess.Cols()+c,pInitialGuess(r,c));

//    const gsl_multimin_fdfminimizer_type *lMinType=gsl_multimin_fdfminimizer_conjugate_fr;
    const gsl_multimin_fdfminimizer_type *lMinType=gsl_multimin_fdfminimizer_vector_bfgs;
    gsl_multimin_fdfminimizer *lMinimizer=gsl_multimin_fdfminimizer_alloc(lMinType,pInitialGuess.size());
    gsl_multimin_fdfminimizer_set(lMinimizer,&lFunc,lX,pStepSize,pTolerance);

    msg_info() << "optimizing";
    
    for(int lIter=0;lIter<50;lIter++)
    {
        msg_info() << "." << std::flush;
        int lStatus=gsl_multimin_fdfminimizer_iterate(lMinimizer);
        if(lStatus==27)
        {
            msg_info() << "no more progress after " << lIter << " iterations\n";
            break;
        }
        
        if(lStatus!=GSL_SUCCESS)
            throw XProcess("error "+to_string(lStatus)+"in fdfminimizer_iterate");
        lStatus=gsl_multimin_test_gradient(lMinimizer->gradient,pEbsAbs);
        if(lStatus!=GSL_CONTINUE)
        {
            if(lStatus==GSL_SUCCESS)
            {
                msg_info() << "optimization converged after " << lIter << " iterations\n";
                break;
            }
            else if(lStatus==27)
            {
                msg_info() << "no more progress after " << lIter << " iterations\n";
                break;
            }
                
            throw XProcess("error "+to_string(lStatus)+"in gsl_testgradient");
        }
    }
    
    msg_info() << "\n";

    mat::CMat_d lResult(pInitialGuess.Rows(),pInitialGuess.Cols());
    
    for(int r=0;r<lResult.Rows();r++)
        for(int c=0;c<lResult.Cols();c++)
            lResult(r,c)=gsl_vector_get(lMinimizer->x,r*lResult.Cols()+c);

    msg_info() << "final optimization value: " << lMinimizer->f << "\n";

    gsl_multimin_fdfminimizer_free(lMinimizer);
    gsl_vector_free(lX);
    
    return lResult;
}
    
/*namespace mat*/ } /*namespace buola*/ }

#endif
